/*
	aco-netdesign -- Ant colony optimization solution to network design problem

	Copyright (C) 2008 Jeffrey Sharkey, http://jsharkey.org/
	
	Developed by Jeffrey Sharkey as part of his thesis work at Montana State
	University. His work was sponsored by the Western Transportation Institute,
	and was guided by advisor Doug Galarus. Other valuable guidance was
	provided by Dr. Bill Jameson and Gary Schoep. 

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.
	
	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.
	
	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "graph.h"





#include <pthread.h>
#include <queue>

queue<Vertex*> tasks;
pthread_mutex_t mutexqueue = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t mutexsave = PTHREAD_MUTEX_INITIALIZER;

Graph* graph;
TimeRemaining* t;

#define THREADS 8

void *thread_func(void *vptr_args) {

while(true) {
		
// pull work item off the queue or terminate if empty
if(tasks.empty()) break;
		
pthread_mutex_lock(&mutexqueue);
Vertex* v = tasks.front();
tasks.pop();
pthread_mutex_unlock(&mutexqueue);

// perform actual processing
ulist<Edge*> result = graph->adjacent(v);


pthread_mutex_lock(&mutexsave);
graph->adjacentLookup[v] = result;
pthread_mutex_unlock(&mutexsave);


t->increment();

}
	
return NULL;
	
}








int Tagged::nextid = 0;



Vertex::Vertex(double lat_, double lon_, double cost_) : layer(-1), cost(cost_), bandwidth(0), radius(0), lat(lat_), lon(lon_), terminal(false), root(false), coverage(""), pref(0.5) {
}

Vertex::Vertex(int layer_, double cost_, double bandwidth_, double lat_, double lon_, bool terminal_, bool root_) : layer(layer_), cost(cost_), bandwidth(bandwidth_), radius(0), lat(lat_), lon(lon_), terminal(terminal_), root(root_), pref(0.5) {
}

Vertex::~Vertex() {
}


Edge::Edge(Vertex* u_, Vertex* v_, double cost_) : u(u_), v(v_), cost(cost_), bandwidth(MAX_BANDWIDTH) {
	assert(u_ != NULL && v_ != NULL);
	id = nextid++;
}

Edge::Edge(Vertex* u_, Vertex* v_, double cost_, double bandwidth_) : u(u_), v(v_), cost(cost_), bandwidth(bandwidth_) {
	assert(u_ != NULL && v_ != NULL);
	id = nextid++;
}

Edge::Edge(Vertex* u_, Vertex* v_, double cost_, double bandwidth_, double margin_) : u(u_), v(v_), cost(cost_), bandwidth(bandwidth_), margin(margin_) {
	assert(u_ != NULL && v_ != NULL);
	id = nextid++;
}

Edge::~Edge() {
}


bool Edge::contains(Vertex* w) {
	assert(w != NULL);
	return (*w == *u || *w == *v);
}

Vertex* Edge::adjacent(Vertex* w) {
	if(*w == *u) return v;
	if(*w == *v) return u;
	return NULL;
}



Path::Path(Vertex* start_, Vertex* end_) : start(start_), end(end_), invalid(false) {
//assert(start_ != NULL && end_ != NULL);
	current = start;
	vertexes.insert(current);
}

Path::~Path() {
}

void Path::follow(Edge* e) {
	current = e->adjacent(current);
	assert(current != NULL);
	vertexes.insert(current);
	edges.insert(e);
}

void Path::merge(Path* p) {
	// include the given path into our solution
	vertexes.merge(p->vertexes);
	edges.merge(p->edges);
}

bool Path::finished() {
	return *current == *end;
}

double Path::cost() {
	if(invalid) return LARGE_COST;
	double cost = 0;
	
//cout << "cost() with |V|=" << vertexes.size() << endl;
	list<Vertex*>::iterator v;
	for(v = vertexes.begin(); v != vertexes.end(); v++)
		cost += ((Vertex*)(*v))->cost;
	
	list<Edge*>::iterator e;
	for(e = edges.begin(); e != edges.end(); e++)
		cost += ((Edge*)(*e))->cost;
	
	return cost;
}

Edge* Path::findEdge(Graph* g, Vertex* v) {
	// use prebuilt table from graph to find the first edge that we have
	ulist<Edge*> outgoing = g->adjacent(v);
	ulist<Edge*>::iterator it;
	for(it = outgoing.begin(); it != outgoing.end(); it++) {
		Edge* e = *it;
		if(edges.contains(e)) return e;
	}
	return NULL;
}


void Path::serialize() {
	// dump current path object to console
	cout << vertexes.size() << endl;
	ulist<Vertex*>::iterator i;
	for(i = vertexes.begin(); i != vertexes.end(); i++) {
		Vertex* v = (Vertex*)(*i);
		cout << v->id << endl;
	}
	
	cout << edges.size() << endl;
	ulist<Edge*>::iterator j;
	for(j = edges.begin(); j != edges.end(); j++) {
		Edge* e = (Edge*)(*j);
		cout << e->id << endl;
	}
}

void Path::deserialize(Graph* g) {
	// using the given graph, inflate this path
	int n, m, id;
	cin >> n;
	for(int i = 0; i < n; i++) {
		cin >> id;
		vertexes.insert(g->vertexes[id]);
	}
	
	cin >> m;
	for(int i = 0; i < m; i++) {
		cin >> id;
		edges.insert(g->edges[id]);
	}
}




Graph::Graph() {
	root = new Vertex(-1, -1, 0);
	root->root = true;
	root->terminal = true;
	addVertex(root);
	adjacentBuilt = false;
}

Graph::~Graph() {
//	delete root;

	map<int,Vertex*>::iterator i;
	for(i = vertexes.begin(); i != vertexes.end(); i++) {
		Vertex* v = (Vertex*)(*i).second;
		delete v;
	}
	
	map<int,Edge*>::iterator j;
	for(j = edges.begin(); j != edges.end(); j++) {
		Edge* e = (Edge*)(*j).second;
		delete e;
	}
}







void Graph::buildAdjacent() {

graph = this;
t = new TimeRemaining(vertexes.size(), 32);

map<int,Vertex*>::iterator it;
for(it = vertexes.begin(); it != vertexes.end(); it++) {
	Vertex* v = (Vertex*)(*it).second;
	tasks.push(v);
}

pthread_t thread[THREADS];
//cout << "buildAdjacent: creating threads" << endl;
for(int i = 0; i < THREADS; i++) {
	pthread_create(&thread[i], NULL, &thread_func, NULL);
}
	
	
//cout << "buildAdjacent: joining threads" << endl;
for(int i = 0; i < THREADS; i++) {
	pthread_join(thread[i], NULL);
}

//cout << "buildAdjacent: threads done" << endl;

adjacentBuilt = true;
delete t;



/*
	TimeRemaining* t = new TimeRemaining(vertexes.size(), 32);
	map<int,Vertex*>::iterator it;
	for(it = vertexes.begin(); it != vertexes.end(); it++) {
		Vertex* v = (Vertex*)(*it).second;
		adjacentLookup[v] = adjacent(v);
		t->increment();
	}
	adjacentBuilt = true;
	delete t;
*/
}

void Graph::addVertex(Vertex* v) {
	//vertexes.insert(v);
	vertexes[v->id] = v;
	if(v->terminal)
		terminal.insert(v);
	else
		steiner.insert(v);
}

void Graph::addEdge(Edge* e) {
	edges[e->id] = e;
}

ulist<Edge*> Graph::adjacent(Vertex* v) {
	if(adjacentBuilt)
		return adjacentLookup[v];
	
	// find all edges that contain given vertex
	ulist<Edge*> adjacent;
	map<int,Edge*>::iterator it;
	for(it = edges.begin(); it != edges.end(); it++) {
		Edge* e = (Edge*)(*it).second;
		if(e->contains(v))
			adjacent.insert(e);
	}
	return adjacent;
}

bool Graph::edgeExists(Vertex* u, Vertex* v) {
	// check to see if edge exists between two points
	ulist<Edge*> outgoing = adjacent(u);
	ulist<Edge*>::iterator it;
	for(it = outgoing.begin(); it != outgoing.end(); it++) {
		Edge* e = (*it);
		if(e->contains(v))
			return true;
	}
	return false;
}

void Graph::serializeCode(string filename) {
	// dump the current graph out to actual code
	assert(false);
	/*
	ofstream out(filename.c_str());
	
	out << "Graph* deserializeGraph() {" << endl;
	out << "Graph* g = new Graph();" << endl;

	out << "Vertex* v;" << endl;
	map<int,Vertex*>::iterator i;
	for(i = vertexes.begin(); i != vertexes.end(); i++) {
		Vertex* v = (Vertex*)(*i).second;
		out << "v = new Vertex(" << v->layer << ", " << v->cost << ", " << v->bandwidth << ", " << v->lat << ", " << v->lon << ", " << v->terminal << ", " << v->root << "); v->id = " << v->id << "; g->addVertex(v);" << endl;
	}

	out << "Edge* e;" << endl;
	map<int,Edge*>::iterator j;
	for(j = edges.begin(); j != edges.end(); j++) {
		Edge* e = (Edge*)(*j).second;
		out << "e = new Edge(g->vertexes[" << e->u->id << "], g->vertexes[" << e->v->id << "], " << e->cost << ", " << e->bandwidth << "); e->id = " << e->id << "; g->addEdge(e);" << endl;
	}
	
	out << "return g;" << endl;
	out << "}" << endl;
	*/
}




Lookup::Lookup(Graph* g_) : g(g_), filled(false) {
	width = g->vertexes.size();
	size = (int)pow(width+2,2);
	table = new double[size];
}

Lookup::~Lookup() {
	delete[] table;
}

void Lookup::set(int i, int j, double value) {
	table[i + (j * width)] = value;
}

void Lookup::set(Vertex* u, Vertex* v, double value) {
	set(u->id, v->id, value);
}

double Lookup::get(int i, int j) {
	return table[i + (j * width)];
}

double Lookup::get(Vertex* u, Vertex* v) {
	assert(filled == true);
	return get(u->id, v->id);
}

void Lookup::load(istream& in) {
	//ifstream in(filename.c_str(), ifstream::in);
	double value;
	for(int i = 0; i < width; i++) {
		for(int j = 0; j < width; j++) {
			in >> value;
			set(i, j, value);
		}
	}
	//in.close();
	filled = true;
}

void Lookup::save(ostream& out) {
	// dump out entire table to raw file
	assert(filled == true);
	//ofstream out(filename.c_str(), ofstream::out | ofstream::trunc);
	for(int i = 0; i < width; i++) {
		for(int j = 0; j < width; j++)
			out << "\t" << get(i, j);
		out << endl;
	}
	//out.close();
}

Path* Lookup::shortestPath(Vertex* start, Vertex* end) {
	Path* p = new Path(start, end);
	while(!p->finished()) {
		// find next best step to take from current vertex
		ulist<Edge*> adjacent = g->adjacent(p->current);
		assert(adjacent.size() > 0);
		
		Edge* best = NULL;
		double bestScore = LARGE_COST;
		
		ulist<Edge*>::iterator it;
		for(it = adjacent.begin(); it != adjacent.end(); it++) {
			Edge* e = (Edge*)(*it);
			Vertex* consider = e->adjacent(p->current);
			
			// check if going back down to roadway, or already visited vertex
			if(consider->layer == 3) continue;
			if(p->vertexes.contains(consider)) continue;
			
			double score = get(consider, end);
			if(score < bestScore) {
				best = e;
				bestScore = score;
			}
			
		}
		
		assert(best != NULL);
		p->follow(best);
		
	}
	return p;
}




// type 1 is roadside end-to-end
// type 2 is roadside end-to-end with full roadway coverage
// type 3 is full roadway coverage with roadside and backbone towers

Graph* buildType3(istream& in, int min, int max, int total) {
	Graph* g = new Graph();
	
	// grab and insert root vertex (backbone)
	Vertex* root = g->root;
	root->layer = 0;
	
	Vertex** backbone = new Vertex*[total];
	Vertex** roadside = new Vertex*[total];
	Vertex** terminal = new Vertex*[total];
	
	int i = -1;
	int minBackbone = -1, maxBackbone = -1;
	bool terminalMode = true;
	
	double lat, lon;
	string coverage;
	while(in.good()) {
		i++;
		in >> lat >> lon >> coverage;
		
		// switch over to backbone mode in file
		// or quit if already in backbone mode
		if(lat == -1) {
			if(terminalMode == false) break;
			terminalMode = false;
			continue;
		}
		
		Vertex* v;
		if(terminalMode) {
			// skip all terminal nodes outside of range
			if(i < min || i >= max) continue;
			
			// create the roadway node
			v = new Vertex(lat, lon, 0);
			v->terminal = true;
			v->layer = 3;
			v->coverage = coverage;
			terminal[i] = v;
			g->addVertex(v);
			
			// create the roadside relay node
			v = new Vertex(lat, lon, 2);
			v->layer = 2;
			v->coverage = coverage;
			roadside[i] = v;
			g->addVertex(v);
			
		} else {
			// record bounds on backbone towers
			if(minBackbone == -1)
				minBackbone = i;
			maxBackbone = i+1;
			
			// insert all backbone towers
			v = new Vertex(lat, lon, 10);
			v->layer = 1;
			v->coverage = coverage;
			backbone[i] = v;
			g->addVertex(v);
			
		}
		
	}
	//in.close();
	
	for(i = minBackbone; i < maxBackbone; i++) {
		// connect backbone nodes to root
		Vertex* v = backbone[i];
		g->addEdge(new Edge(v, root, 0.01));
		for(int j = min; j < max; j++) {
			// connect backbone to roadside if coverage
			Vertex* w = roadside[j];
			if(v->coverage[j] == '1')
				g->addEdge(new Edge(v, w, 5));
		}
	}
	
	for(i = min; i < max; i++) {
		Vertex* v = roadside[i];
		for(int j = min; j < max; j++) {
			// connect roadside to roadway
			Vertex* w = terminal[j];
			if(v->coverage[j] == '1')
//				g->addEdge(new Edge(v, w, 0.001 * ((double)rand() / (double)RAND_MAX)));
				g->addEdge(new Edge(v, w, 0.001 * mt_rand()));
			
			// connect roadside with other relays
			if(i == j) continue;
			w = roadside[j];
			if(v->coverage[j] == '1')
				g->addEdge(new Edge(v, w, 1));
		}
	}
	
	delete[] backbone;
	delete[] roadside;
	delete[] terminal;
	
	return g;
}




Lookup* buildFloyd(Graph* g) {
	// build a floyd lookup table for given graph
	Lookup* look = new Lookup(g);
	int n = look->width;
	
	// fill table with default costs
	for(int i = 0; i < n; i++) {
		for(int j = 0; j < n; j++) {
			look->set(i, j, (i == j) ? 0 : LARGE_COST);
		}
	}
	
	// fill table with initial known edge costs
	// ignore edges going DOWN to roadway layer to prevent loops
	map<int,Edge*>::iterator it;
	for(it = g->edges.begin(); it != g->edges.end(); it++) {
		Edge* e = (Edge*)(*it).second;
		if(e->u->layer != 3) look->set(e->v, e->u, e->cost + e->v->cost);
		if(e->v->layer != 3) look->set(e->u, e->v, e->cost + e->u->cost);
	}
	

	// fill remaining table with floyds algorithm
	TimeRemaining* t = new TimeRemaining(n, 32);
	double value;
	for(int k = 0; k < n; k++) {
		for(int i = 0; i < n; i++) {
			for(int j = 0; j < n; j++) {
				value = look->get(i, k) + look->get(k, j);
				if(look->get(i, j) > value) {
					look->set(i, j, value);
				}
			}
		}
		t->increment();
	}

	
	// layer on finishing vertex cost for all active paths
	for(int j = 0; j < n; j++) {
		//Vertex* v = find(g->vertexes, j);
		Vertex* v = g->vertexes[j];
		assert(v != NULL);
		for(int i = 0; i < n; i++) {
			if(look->get(i, j) == LARGE_COST) continue;
			look->set(i, j, look->get(i, j) + v->cost);
		}
	}
	
	look->filled = true;
	return look;
	
}


Vertex* find(ulist<Vertex*> vertexes, int id) {
	ulist<Vertex*>::iterator it;
	for(it = vertexes.begin(); it != vertexes.end(); it++) {
		Vertex* v = (Vertex*)(*it);
		if(v->id == id) return v;
	}
	return NULL;
}

Edge* find(ulist<Edge*> edges, int id) {
	ulist<Edge*>::iterator it;
	for(it = edges.begin(); it != edges.end(); it++) {
		Edge* e = (Edge*)(*it);
		if(e->id == id) return e;
	}
	return NULL;
}


/*
bool contains(list<Vertex*> vertexes, Vertex* v) {
	list<Vertex*>::iterator it;
	for(it = vertexes.begin(); it != vertexes.end(); it++)
		if(*v == **it) return true;
	return false;
}

bool contains(list<Edge*> edges, Edge* e) {
	list<Edge*>::iterator it;
	for(it = edges.begin(); it != edges.end(); it++)
		if(*e == **it) return true;
	return false;
}
*/

